#include "motor.h"
#include "../hardware/io.h"
#include "sys_timer.h"
#include "hardware/pwm.h"
#include "../hardware/tim_encode.h"

void MotorInit() {
    IODisableJtag();
    IOInit(AIN_L_1, Mode_Out_PP);
    IOInit(AIN_L_2, Mode_Out_PP);
    IOInit(AIN_R_1, Mode_Out_PP);
    IOInit(AIN_R_2, Mode_Out_PP);

    // IOInit(PWM_L_PIN, Mode_Out_PP);
    // IOInit(PWM_R_PIN, Mode_Out_PP);
    PwmInit(PWM_L_PIN, MOTOR_MAX_OUT * MOTOR_CTRL_FREQ, MOTOR_MAX_OUT);
    PwmInit(PWM_R_PIN, MOTOR_MAX_OUT * MOTOR_CTRL_FREQ, MOTOR_MAX_OUT);
    TimEncodeInit(ENCODE_L);
    TimEncodeInit(ENCODE_R);
}

void MotorBrake(uint8_t ain1, uint8_t ain2) {
    IOWrite(ain1, 1);
    IOWrite(ain2, 1);
}

void MotorFreeStop(uint8_t ain1, uint8_t ain2) {
    IOWrite(ain1, 0);
    IOWrite(ain2, 0);
}

void MotorForward(uint8_t ain1, uint8_t ain2) {
    IOWrite(ain1, 1);
    IOWrite(ain2, 0);
}

void MotorBackward(uint8_t ain1, uint8_t ain2) {
    IOWrite(ain1, 0);
    IOWrite(ain2, 1);
}

void MotorStatusCtrl(MOTOR_STATUS_E status, uint8_t ain1, uint8_t ain2) {
    switch (status) {
        case MOTOR_FORWARD:
            MotorForward(ain1, ain2);
            break;
        case MOTOR_BACKWARD:
            MotorBackward(ain1, ain2);
            break;
        case MOTOR_FREE_STOP:
            MotorFreeStop(ain1, ain2);
            break;
        case MOTOR_BRAKE:
            MotorBrake(ain1, ain2);
            break;
    }
}

void MotorOut(MOTOR_SELECT_E motor, MOTOR_STATUS_E status, uint32_t out) {
    out = out < MOTOR_MAX_OUT ? out : (MOTOR_MAX_OUT - 1);
    if (motor == MOTOR_LEFT || motor == MOTOR_ALL) {
        MotorStatusCtrl(status, AIN_L_1, AIN_L_2);
        PwmSetPulse(PWM_L_PIN, out);
    }
    if (motor == MOTOR_RIGHT || motor == MOTOR_ALL) {
        MotorStatusCtrl(status, AIN_R_1, AIN_R_2);
        PwmSetPulse(PWM_R_PIN, out);
    }
}

int32_t MotorGetSpeedL() {
    return TimEncodeGet(ENCODE_L);
}

int32_t MotorGetSpeedR() {
    return TimEncodeGet(ENCODE_R);
}


void MotorTest() {
    volatile uint32_t e_l, e_r;
    MotorInit();
    MotorOut(MOTOR_LEFT, MOTOR_FORWARD, 72);
    MotorOut(MOTOR_RIGHT, MOTOR_FORWARD, 300);
    while (1) {
        DelayMs(1000);
        e_l = TimEncodeGet(ENCODE_L);
        e_r = TimEncodeGet(ENCODE_R);
    }

}